/*
 * caosensornode.h
 *
 *
 *  Created on: Oct 8, 2011
 *  Author: Mario H. Voorsluys
 *	email: forcaeluz@gmail.com
 *
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, visit:
 * http://www.gnu.org/copyleft/gpl.html
 */

//TODO: Implement directives to automatize sensors customization.
#ifndef ROSNODE_H_
#define ROSNODE_H_

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include "SerialReader.h"

class CaoSensorNode {
public:
	/**
	 * Node constructor.
	 * This method actually only calls the init method.
	 */
	CaoSensorNode();
	/**
	 * Node destructor.
	 * Shuts down this ros node.
	 */
	virtual ~CaoSensorNode();
	/**
	 * Initializes the node. All the publishers are advertized, and the serial communication is started.
	 */
	void init();
	/**
	 * This method is where the node actually works.
	 * The sensor data is read, and published to the channels. Currently error values are interpreted by the SerialReader, and only transmitted
	 * as a -1 value to the node, which emits a ROS_WARNING.
	 */
	void spin();

protected:


	// Message Queue size: 1000. Now it's a constant. It could be a compiler directive.
	static const int MESSAGE_QUEUE_SIZE = 1000;

	/*
	 * The sonars are the sensors commanded using the TWI/I2C protocol. This is implemented in the board side of the code.
	 * This code is for three sensors. Expand or reduce it according to your needs.
	 */
	// Change the addresses to desired values.
	unsigned char sonar1_addr;
	unsigned char sonar2_addr;
	unsigned char sonar3_addr;
	// The Handle for this node.
	ros::NodeHandle node_handle;
	// The data publisher for the first sonar.
	ros::Publisher sonar1_publisher;
	// The data publisher for the second sonar.
	ros::Publisher sonar2_publisher;
	// The data publisher for the third sonar.
	ros::Publisher sonar3_publisher;

	/*
	 * The IR sharp sensors could be read through phidgets, or trought the ADC converters from the CAO board.
	 * The second option is less desirable, since it's known that the board has relatively spoken a lot of noise, turning the ADC
	 * values less reliable.
	 * But if for some reason it's desirable to use the sensors on this board, enable the sharp sensors.
	 * Here the example for 4 sensors. Expand, or reduce it acoording to your needs.
	 */
	// The data publisher for the first IR sharp sensor
	ros::Publisher sharp1_publisher;
	// The data publisher for the first IR sharp sensor
	ros::Publisher sharp2_publisher;
	// The data publisher for the first IR sharp sensor
	ros::Publisher sharp3_publisher;
	// The data publisher for the first IR sharp sensor
	ros::Publisher sharp4_publisher;
	// The serial communicator.
	SerialReader communicator;
};

#endif /* ROSNODE_H_ */
